#!/usr/bin/python3 python
# encoding: utf-8
import cv2
import yaml
import numpy as np

K = [(7.1846572637704753e+02)/2, 0., (6.6807583495910785e+02)/2
    , 0.,(7.1817067716228905e+02)/2, (3.9526287993107701e+02)/2, 0., 0., 1.]
R = [ 9.9998858955022607e-01, -6.1741420961919282e-04,
       4.7370422251780363e-03, 6.2100929014201767e-04,
       9.9999952027349182e-01, -7.5749603801350768e-04,
       -4.7365722638757126e-03, 7.6042914187263329e-04,
       9.9998849324915196e-01 ]
D = [ -6.1837379631411421e-02, 1.5311894062098848e-02,
       -4.5557679918291155e-02, 2.2918191465033134e-02 ]
P = [ 6.2634409373262474e+02/2, 0., 6.6052319379823757e+02/2, 0., 0.,
       6.2634409373262474e+02/2, 4.1507166940597665e+02/2, 0., 0., 0., 1.,
       0. ]

W, H = 640, 400

def read_yaml(yaml_path):
    # file = open(yaml_path, 'r', encoding='utf-8')
    # cont = file.read()
    # yaml_data = yaml.load(cont)
    # file.close()
    # print(yaml_data['cameras'])

    fs = cv2.FileStorage(yaml_path, cv2.FILE_STORAGE_READ)

    camera_data = fs.getNode('cameras')
    # print(camera_data.isString())
    # print(camera_data.isMap())
    # print(camera_data.isNone())
    # print(camera_data.isNamed())
    # print(camera_data.isSeq())

    default = []
    for i in range(camera_data.size()):
        v = camera_data.at(i)
        # print(v.isString())
        # print(v.isMap())
        # print(v.isNone())
        # print(v.isNamed())
        # print(v.isSeq())
        key_data = v.keys()
        print(key_data)
        for key in key_data:
            if key == 'image_dimension':
                dim_data = v.getNode(key)

                for j in range(dim_data.size()):
                    v_dim = dim_data.at(j)
                    if v_dim.isInt():

                        default.append(int(v_dim.real()))
                    elif v_dim.isReal():
                        default.append(v_dim.real())
                print(default)
    # cv2.FileNode.
    if default[0] == 640 and default[1]==400:
        remap_r = fs.getNode('Rl').mat()
        remap_k = fs.getNode('Kl').mat()
        remap_d = fs.getNode('Dl').mat()
        remap_p = fs.getNode('Pl').mat()
    else:
        remap_r = fs.getNode('Rl').mat()
        remap_k = fs.getNode('Kl').mat()/2
        remap_d = fs.getNode('Dl').mat()
        remap_p = fs.getNode('Pl').mat()/2
        remap_k[-1][-1] = 1.0
        remap_p[-1][-2] = 1.0

    fisheye_init_x, fisheye_init_y = np.ndarray((H, W), np.float32), np.ndarray((H, W), np.float32)
    cv2.fisheye.initUndistortRectifyMap(remap_k, remap_d, remap_r, remap_p[0:3, 0:3], (W, H), cv2.CV_32FC1, fisheye_init_x, fisheye_init_y)
    return fisheye_init_x, fisheye_init_y

def ReadPara():
    k = np.reshape(K, (3, 3))
    d = np.reshape(D, (4, 1))
    r = np.reshape(R, (3, 3))
    p = np.reshape(P, (3, 4))

    fisheye_x, fisheye_y = np.ndarray((H, W), np.float32), np.ndarray((H, W), np.float32)

    cv2.fisheye.initUndistortRectifyMap(k, d, r, p[0:3, 0:3], (W, H), cv2.CV_32FC1, fisheye_x, fisheye_y )

    return fisheye_x, fisheye_y

fisheye_x, fisheye_y = ReadPara()

def Remap(image):
    imgaeRemap = cv2.remap(image, fisheye_x, fisheye_y, cv2.INTER_LINEAR)
    return imgaeRemap


def remap_eye(image, fisheye_init_x, fisheye_init_y):
    imgae_remap = cv2.remap(image, fisheye_init_x, fisheye_init_y, cv2.INTER_LINEAR)
    return imgae_remap

